/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "canbusservodriver.h"
#include <QDebug>
#include <QString>

#define NSEC_PER_SEC 1000000000
#define D_AMC_SPEED_RATIO 6.5536

CanbusServoDriver::CanbusServoDriver(QString configFileIn, int aliasIn, int busNumberIn,
                                     int deviceIdIn, CanbusDeviceDriver *canbusDriverIn)
{
//    tcpVrep=NULL;
    isInitialedOk=false;
    alias=aliasIn;
    busNumber=busNumberIn;
    deviceId=deviceIdIn;
    canbusDriver=canbusDriverIn;

    controlWord=eServoOff;
    isUpdateSucess=true;
    errorCode=0;
    statusWord=0;
    speedFeedback=0;
    batteryVoltage=10000;
    batteryVolume=1;

    errorCode_channel2=0;
    statusWord_channel2=0;
    speedFeedback_channel2=0;

    pulsePerRound_channel1=10000;
    pulsePerRound_channel2=10000;


    initial(configFileIn);

}

CanbusServoDriver::~CanbusServoDriver()
{
//    if(  tcpVrep!=NULL )
//    {
//        tcpVrep->close();
    //    }
}

int CanbusServoDriver::getBatteryInfo(BatteryInformation &batteryInfoOut)
{
    batteryInfoOut.errorStatus=0;
    batteryInfoOut.batteryCurrentCurrency=0;
    batteryInfoOut.batterySumVoltage=batteryVoltage;
    batteryInfoOut.batteryLeftLightmodRatio=batteryVolume;

    return 1;
}

int CanbusServoDriver::sendSdoGetCommandFrame()
{
    send_getErrorCode_command();
    realTimeSleep(D_SDO_WAIT_INTERVAL_TIME);
    send_getPositionFeedback_command();
    realTimeSleep(D_SDO_WAIT_INTERVAL_TIME);
    send_getStatusWord_command();
    realTimeSleep(D_SDO_WAIT_INTERVAL_TIME);
    send_getSpeedFeedback_command();
    realTimeSleep(D_SDO_WAIT_INTERVAL_TIME);
    send_getCurrencyFeedback_command();
    realTimeSleep(D_SDO_WAIT_INTERVAL_TIME);
    return 1;
}

int CanbusServoDriver::sendPdoData(unsigned short controlWord, int targetPosition, int targetVelocity,
                                   short targetTorque, double pulsePerRound1)
{
    if( false==isInitialedOk)
    {
        return -1;
    }
//    qDebug()<<"controlWord"<<controlWord;


    int tmpReturn;
    unsigned int pdoId;
    QVector<unsigned char> dataList;

    switch(vendorId)
    {
    case E_VENDOR_ID_KINCO:
    {
        targetVelocity=16.384*targetVelocity;//与编码器线数无关。
        //pdoId 1
        pdoId=1;
        dataList.resize(8);
        dataList[0]= controlWord& 0xff;
        dataList[1]= controlWord  >> 8 ;
        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);

        //pdoId 2
        pdoId=2;
        dataList.resize(8);
        dataList[3]= targetPosition >> 24 ;
        dataList[2]=(targetPosition& 0xff0000)>>16;
        dataList[1]=(targetPosition& 0xff00)>>8;;
        dataList[0]=targetPosition& 0xff;

        dataList[7]= targetVelocity >> 24 ;
        dataList[6]=(targetVelocity& 0xff0000)>>16;
        dataList[5]=(targetVelocity& 0xff00)>>8;;
        dataList[4]=targetVelocity& 0xff;

        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);
        break;
    }
    case E_VENDOR_ID_MOTEC:
    {
//        targetVelocity=60*targetVelocity/10000.0;//motec采用r/min。电机编码器２５００puls/r
        if(pulsePerRound1>0)
        {
            pulsePerRound_channel1=pulsePerRound1;
//            qDebug()<<"set pulsePerRound_channel1"<<pulsePerRound_channel1;
            targetVelocity=60*targetVelocity/pulsePerRound1;//pulse/s转为r/min。行走轮电机直接使用１０２４线编码器
        }
        else
        {
            targetVelocity=0;
//            qDebug()<<"error ,,,pulsePerRound1="<<pulsePerRound1;
        }
        //pdoId 1
        pdoId=1;
        dataList.resize(8);
        dataList[0]= controlWord& 0xff;
        dataList[1]= controlWord  >> 8 ;
        dataList[3]=(targetVelocity& 0xff00)>>8;//正负号问题徐解决。
        dataList[2]=targetVelocity& 0xff;

        dataList[7]= targetPosition >> 24 ;
        dataList[6]=(targetPosition& 0xff0000)>>16;
        dataList[5]=(targetPosition& 0xff00)>>8;;
        dataList[4]=targetPosition& 0xff;

        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);

        //pdoId 2
//        pdoId=2;
//        dataList.resize(8);
////        dataList[3]= targetPosition >> 24 ;
////        dataList[2]=(targetPosition& 0xff0000)>>16;
////        dataList[1]=(targetPosition& 0xff00)>>8;
////        dataList[0]=targetPosition& 0xff;

////        dataList[5]=(targetVelocity& 0xff00)>>8;//正负号问题徐解决。
////        dataList[4]=targetVelocity& 0xff;

//        dataList[1]=(targetVelocity& 0xff00)>>8;//正负号问题徐解决。
//        dataList[0]=targetVelocity& 0xff;

//        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);
        break;
    }
    case E_VENDOR_ID_ROBOTIQ:
    {//前面低字节、后面高字节.P182,P218 EX 急停　，MS 普通停止，　MG　急停复位。FF　错误标记，ＦＳ状态标记，ＦＭ电机状态标记
//        targetVelocity=60*targetVelocity/1024.0;//pulse/s转为r/min。舵轮使用１０２４线编码器
        if(pulsePerRound1>0)
        {
            pulsePerRound_channel1=pulsePerRound1;
//            qDebug()<<"set pulsePerRound_channel1"<<pulsePerRound_channel1;
            targetVelocity=60*targetVelocity/pulsePerRound1;//pulse/s转为r/min。行走轮电机直接使用１０２４线编码器
        }
        else
        {
            targetVelocity=0;
//            qDebug()<<"error ,,,pulsePerRound1="<<pulsePerRound1;
        }
        //pdoId 1
        pdoId=1;
        dataList.resize(8);

        //controlWord 0 switch one, 1 powerEnable,2 quickstop,3 enableOperation,4-6 operationMode,7 faultReset,8 hault.
        dataList[0]= controlWord& 0xff;
        dataList[1]= controlWord  >> 8 ;
        dataList[2]=0;
        dataList[3]=0;

//        dataList[7]= targetPosition >> 24 ;
//        dataList[6]=(targetPosition& 0xff0000)>>16;
//        dataList[5]=(targetPosition& 0xff00)>>8;;
//        dataList[4]=targetPosition& 0xff;

        dataList[7]=0;
        dataList[6]=0;
        dataList[5]=0;
        dataList[4]=0;
        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);

        //pdoId 2
        pdoId=2;
        dataList.resize(8);

        dataList[3]= targetVelocity >> 24 ;
        dataList[2]=(targetVelocity& 0xff0000)>>16;
        dataList[1]=(targetVelocity& 0xff00)>>8;;
        dataList[0]=targetVelocity& 0xff;
        static char robotiqHeartbeat=0;
        robotiqHeartbeat++;
        dataList[4]=robotiqHeartbeat;
        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);

        break;

    }
    case E_VENDOR_ID_AMC://单位换算查看说明书ｐａｇｅ290
    {
        //可能ａmc设置软件上面只能激活１个通道，或设置上电默认命令源为none，或motion engine shutdown
        //Rpod好像是固定不可修改的。速度模式和位置模式需要用不同的pdo通道. 位置用pdo1 速度用pdo2
        targetVelocity=D_AMC_SPEED_RATIO*targetVelocity;
        //pdoId 1
        pdoId=1;//实际发送id=200+pdoId ,300+pdoId,400+pdoId,500+pdoId
        dataList.resize(8);
        dataList[0]= controlWord& 0xff;
        dataList[1]= controlWord  >> 8 ;

        dataList[5]= targetPosition >> 24 ;
        dataList[4]=(targetPosition& 0xff0000)>>16;
        dataList[3]=(targetPosition& 0xff00)>>8;;
        dataList[2]=targetPosition& 0xff;

        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);

        //pdoId 2
        pdoId=2;
        dataList.resize(8);

        dataList[0]= controlWord& 0xff;
        dataList[1]= controlWord  >> 8 ;

        dataList[5]= targetVelocity >> 24 ;
        dataList[4]=(targetVelocity& 0xff0000)>>16;
        dataList[3]=(targetVelocity& 0xff00)>>8;
        dataList[2]=targetVelocity& 0xff;

        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);
        break;

    }

    }




    return tmpReturn;
}

int CanbusServoDriver::sendPdoData_twoMotor(unsigned short controlWord1, int targetPosition1, int targetVelocity1,
                                            short targetTorque1, double pulsePerRound1,
                                            unsigned short controlWord2, int targetPosition2, int targetVelocity2,
                                            short targetTorque2, double pulsePerRound2)
{
    if( false==isInitialedOk)
    {
        return -1;
    }
//    qDebug()<<"controlWord"<<controlWord;


    int tmpReturn=-1;
    unsigned int pdoId;
    QVector<unsigned char> dataList;

    switch(vendorId)
    {
    case E_VENDOR_ID_ROBOTIQ2MOTOR:
    {//前面低字节、后面高字节.P182,P218 EX 急停　，MS 普通停止，　MG　急停复位。FF　错误标记，ＦＳ状态标记，ＦＭ电机状态标记
        if(pulsePerRound1>0)
        {
            pulsePerRound_channel1=pulsePerRound1;
//            qDebug()<<"set pulsePerRound_channel1"<<pulsePerRound_channel1;
            targetVelocity1=60*targetVelocity1/pulsePerRound1;//pulse/s转为r/min。行走轮电机直接使用１０２４线编码器
        }
        else
        {
            targetVelocity1=0;
//            qDebug()<<"error ,,,pulsePerRound1="<<pulsePerRound1;
        }
        if(pulsePerRound2>0)
        {
            pulsePerRound_channel2=pulsePerRound2;
//            qDebug()<<"set pulsePerRound_channel2"<<pulsePerRound_channel2;
            targetVelocity2=60*targetVelocity2/pulsePerRound2;//pulse/s转为r/min。舵轮使用１０２４线编码器
        }
        else
        {
             targetVelocity2=0;
//            qDebug()<<"error ,,,pulsePerRound2="<<pulsePerRound2;
        }


        //pdoId 1
        pdoId=1;
        dataList.resize(8);

        //controlWord 0 switch one, 1 powerEnable,2 quickstop,3 enableOperation,4-6 operationMode,7 faultReset,8 hault.
        dataList[0]= controlWord1& 0xff;
        dataList[1]= controlWord1  >> 8 ;
        dataList[2]=0;
        dataList[3]=0;

        dataList[7]=0;
        dataList[6]=0;
        dataList[5]=0;
        dataList[4]=0;
        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);

        //pdoId 2
        pdoId=2;
        dataList.resize(8);

        dataList[3]= targetVelocity1 >> 24 ;
        dataList[2]=(targetVelocity1& 0xff0000)>>16;
        dataList[1]=(targetVelocity1& 0xff00)>>8;;
        dataList[0]=targetVelocity1& 0xff;
        static char robotiqHeartbeat=0;
        robotiqHeartbeat++;
        dataList[4]=robotiqHeartbeat;
        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);

        //pdoId 3
        pdoId=3;
        dataList.resize(8);

        dataList[0]=0;
        dataList[1]=0;
        dataList[2]=0;
        dataList[3]=0;

        dataList[7]= targetPosition2 >> 24 ;
        dataList[6]=(targetPosition2 & 0xff0000)>>16;
        dataList[5]=(targetPosition2 & 0xff00)>>8;;
        dataList[4]=targetPosition2 & 0xff;

        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);

        //pdoId 4
        pdoId=4;
        dataList.resize(8);

        dataList[3]= targetVelocity2 >> 24 ;
        dataList[2]=(targetVelocity2& 0xff0000)>>16;
        dataList[1]=(targetVelocity2& 0xff00)>>8;;
        dataList[0]=targetVelocity2& 0xff;

        dataList[7]=0;
        dataList[6]=0;
        dataList[5]=0;
        dataList[4]=0;
        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);

        break;

    }
    case E_VENDOR_ID_XingSong2MOTOR:
    {

        if(0==controlWord1&4)//quick stop
        {
            controlWorld_xinsong=0;
        }
        else if(6==controlWord1&7)//servo off
        {
            controlWorld_xinsong=0;
        }
        else if(15==controlWord1&7)//servo on
        {
            controlWorld_xinsong=1;
        }

        unsigned char resetFault_xinsong;
        if(1==controlWord1&0x80)//reset fault
        {
            resetFault_xinsong=1;
        }
        else
        {
            resetFault_xinsong=0;
        }

        unsigned char motor1Direction,motor2Direction;
        unsigned short motor1Velocity,motor2Velocity;
        unsigned short motor1Position,motor2Position;
        motor1Position=abs(targetPosition1);
        motor2Position=abs(targetPosition2);
        //1号电机速度控制，2号电机位置控制
        if(targetVelocity1>=0)
        {
            motor1Direction=0;
        }
        else
        {
            motor1Direction=1;
        }

        if(targetPosition2>=0)
        {
            motor2Direction=0;
        }
        else
        {
            motor2Direction=1;
        }

        if(pulsePerRound1>0)
        {
            pulsePerRound_channel1=pulsePerRound1;
//            qDebug()<<"set pulsePerRound_channel1"<<pulsePerRound_channel1;
            motor1Velocity=60*targetVelocity1/pulsePerRound1;//pulse/s转为r/min。行走轮电机直接使用１０２４线编码器
        }
        else
        {
            motor1Velocity=0;
//            qDebug()<<"error ,,,pulsePerRound1="<<pulsePerRound1;
        }
        if(pulsePerRound2>0)
        {
            pulsePerRound_channel2=pulsePerRound2;
//            qDebug()<<"set pulsePerRound_channel2"<<pulsePerRound_channel2;
            motor2Velocity=60*targetVelocity2/pulsePerRound2;//pulse/s转为r/min。舵轮使用１０２４线编码器
        }
        else
        {
             motor2Velocity=0;
//            qDebug()<<"error ,,,pulsePerRound2="<<pulsePerRound2;
        }

        motor2Velocity=0;//位置控制


        //pdoId 1
        pdoId=1;
        dataList.resize(8);

        //controlWord 0 switch one, 1 powerEnable,2 quickstop,3 enableOperation,4-6 operationMode,7 faultReset,8 hault.
        dataList[0]= controlWorld_xinsong;
        dataList[1]= motor1Direction ;
        dataList[2]= motor1Velocity& 0xff;
        dataList[3]= motor1Velocity  >> 8 ;
        dataList[4]= motor1Position& 0xff;
        dataList[5]= motor1Position  >> 8 ;

        dataList[6]=0;
        dataList[7]=0;

        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);

        //pdoId 2
        pdoId=2;
        dataList.resize(8);

        dataList[0]= controlWorld_xinsong;
        dataList[1]= motor2Direction ;
        dataList[2]= motor2Velocity& 0xff;
        dataList[3]= motor2Velocity  >> 8 ;
        dataList[4]= motor2Position& 0xff;
        dataList[5]= motor2Position  >> 8 ;

        dataList[6]=0;
        dataList[7]=0;
        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);

        //pdoId 3
        pdoId=3;
        dataList.resize(8);

        dataList[0]=resetFault_xinsong;
        dataList[1]=0;
        dataList[2]=0;
        dataList[3]=0;

        dataList[7]=0;
        dataList[6]=0;
        dataList[5]=0;;
        dataList[4]=0;

        tmpReturn=canbusDriver->sendPdoData(deviceId,busNumber,alias,pdoId,dataList);


        break;

    }
    case E_VENDOR_ID_CURTIS2MOTOR:
    {//前面低字节、后面高字节.速度L,速度H（r/min),角度L,角度H，泵速度L,泵速度H（r/min),下降比例阀L,下降比例阀H(0-32767)
        if(pulsePerRound1>0)
        {
            pulsePerRound_channel1=pulsePerRound1;
//            qDebug()<<"set pulsePerRound_channel1"<<pulsePerRound_channel1;
            targetVelocity1=60*targetVelocity1/pulsePerRound1;//pulse/s转为r/min。行走轮电机直接使用１０２４线编码器
        }
        else
        {
            targetVelocity1=0;
//            qDebug()<<"error ,,,pulsePerRound1="<<pulsePerRound1;
        }
        if(pulsePerRound2>0)
        {
            pulsePerRound_channel2=pulsePerRound2;
//            qDebug()<<"set pulsePerRound_channel2"<<pulsePerRound_channel2;
            targetVelocity2=60*targetVelocity2/pulsePerRound2;//pulse/s转为r/min。舵轮使用１０２４线编码器
        }
        else
        {
             targetVelocity2=0;
//            qDebug()<<"error ,,,pulsePerRound2="<<pulsePerRound2;
        }

        //quickstop
        if(109!=controlWord1)
        {
            targetVelocity1=0;
        }

        //pdoId 1
        pdoId=1;
        dataList.resize(8);

        dataList[1]=(targetVelocity1& 0xff00)>>8;;
        dataList[0]=targetVelocity1& 0xff;
        //位置反馈是经过减速比后的位置。100代表1度。强制设置减速比为1，虚拟编码器线数值为实际值×减速比
        short tmpPosition;
//        targetPosition2=pulsePerRound_channel2*tmpPosition/36000.0;
        tmpPosition=targetPosition2*36000.0/pulsePerRound_channel2;
        if(tmpPosition>9000)
        {
            tmpPosition=9000;
        }
        else if(tmpPosition<-9000)
        {
            tmpPosition=-9000;
        }
        dataList[3]=(tmpPosition & 0xff00)>>8;;
        dataList[2]=tmpPosition & 0xff;

        dataList[7]=0;
        dataList[6]=0;
        dataList[5]=0;
        dataList[4]=0;
        tmpReturn=canbusDriver->sendPdoData_toMaster(deviceId,busNumber,alias,pdoId,dataList);


        break;

    }

    }




    return tmpReturn;
}

int CanbusServoDriver::sendNodeWatchDogFrame()
{
    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
           return 1;

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        tmpReturn=canbusDriver->sendNodeWatchDogFrame(deviceId,busNumber,alias);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    case E_VENDOR_ID_MOTEC:
    {
        tmpReturn=canbusDriver->sendNodeWatchDogFrame(deviceId,busNumber,alias);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }

    default:
    {
        tmpReturn=canbusDriver->sendNodeWatchDogFrame(deviceId,busNumber,alias);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::startPdoMode()
{
    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
           return 1;

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
       E_NODE_MANAGE commandIn=E_NODE_MANAGE_START_PDO;
        tmpReturn=canbusDriver->manageNode(deviceId,busNumber,alias,commandIn);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    case E_VENDOR_ID_MOTEC:
    {
       E_NODE_MANAGE commandIn=E_NODE_MANAGE_START_PDO;
        tmpReturn=canbusDriver->manageNode(deviceId,busNumber,alias,commandIn);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    default:
    {
        E_NODE_MANAGE commandIn=E_NODE_MANAGE_START_PDO;
         tmpReturn=canbusDriver->manageNode(deviceId,busNumber,alias,commandIn);
         if(1==tmpReturn)
         {
             return 1;
         }
         else
         {
             return tmpReturn;
         }
         break;
    }
    }

    return -1;
}

int CanbusServoDriver::setNodeSafePeriodTime(int timeIn)
{
    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }

    int tmpReturn;

    switch(vendorId)
    {
    case E_VENDOR_ID_AI_KONG:
    {
        return -1;
        break;
    }
    case E_VENDOR_ID_KINCO:
    {
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x100c,0,timeIn,2);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    case E_VENDOR_ID_MOTEC:
    {
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x100c,0,timeIn,2);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    case E_VENDOR_ID_ROBOTIQ:
    {
        return 1;
    }
    case E_VENDOR_ID_ROBOTIQ2MOTOR:
    {
        return 1;
    }
    case E_VENDOR_ID_CURTIS2MOTOR:
    {
        return 1;
    }
    case E_VENDOR_ID_XingSong2MOTOR://不支持0x100c
    {
        return 1;
    }
    default:
    {
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x100c,0,timeIn,2);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    }


    return 1;
}

int CanbusServoDriver::initial(QString fileName)
{
    qDebug()<<"CanbusServoDriver initial ,file name="<<fileName;
    driverConfig=new CanbusServoDriverConfig(fileName);
    if(true==driverConfig->isInitialOkStatus())
    {
        int tmpId;
        driverConfig->getVendorId(tmpId);
        vendorId=(E_VENDOR_ID) tmpId;
        driverConfig->getSdoConfig(sdoConfigList);
        isInitialedOk=true;
        return 1;

    }
    return -1;
}

int CanbusServoDriver::process_can_buffer_pdo_curtis()
{
    QVector<unsigned int> nodeIdOut;
    QVector<unsigned int> pdoIdOut;
    QVector<QVector<unsigned char> > dataListOut;

    canbusDriver->getPdoMasterData_ch1(nodeIdOut,pdoIdOut,dataListOut);
//    qDebug()<<"getPdoData----------------------------------*********************************";
//    qDebug()<<"getPdoData nodeIdOut="<<nodeIdOut;
//    qDebug()<<"getPdoData pdoIdOut="<<pdoIdOut;
//    qDebug()<<"getPdoData dataListOut="<<dataListOut;
    bool isReceived=false;
    //前面低字节、后面高字节
    for(int i=0;i<nodeIdOut.size();i++)
    {
        if(alias==nodeIdOut[i])
        {
            if(1==pdoIdOut[i])
            {
                if(8==dataListOut[i].size())
                {
                    //行走errorcode,转向errorcode,速度L,速度H（r/min),角度L,角度H，泵错误代码，电量

                    //暂时不处理电机２的错误
                    errorCode=dataListOut[i][0];
                    //合并电机错误
                    errorCode+=dataListOut[i][1]<<8;

                    errorCode_channel2=errorCode;


                    if(0==errorCode)
                    {
                        statusWord=0x27;//使能状态
                    }
                    else
                    {
                        statusWord=8;//急停外的其它报错
                    }

                    statusWord_channel2=statusWord;

                    currencyFeedback=0;
                    currencyFeedback_channel2=0;

                    batteryVolume=dataListOut[i][7]/100.0;

                    short tmpSpeed=0;
                    tmpSpeed=tmpSpeed|dataListOut[i][2];
                    tmpSpeed=tmpSpeed|(dataListOut[i][3]<<8);

                    speedFeedback=tmpSpeed*pulsePerRound_channel1/60.0;//r/min转为pulse/s。舵轮使用１０２４线编码器
                    speedFeedback_channel2=0;

                    //位置反馈是经过减速比后的位置。100代表1度。强制设置减速比为1，虚拟编码器线数值为实际值×减速比
                    short tmpPosition=0;
                    tmpPosition=tmpPosition|dataListOut[i][4];
                    tmpPosition=tmpPosition|(dataListOut[i][5]<<8);
                    positionFeedback_channel2=pulsePerRound_channel2*tmpPosition/36000.0;

                    positionFeedback=0;

                    isReceived=true;

                }
            }

        }

    }

    return isReceived;

}


void CanbusServoDriver::realTimeSleep(long long umicoroSecond)
{
    struct timespec time;
    clock_gettime(CLOCK_REALTIME,&time);
    time.tv_nsec += umicoroSecond * 1000;
    while(time.tv_nsec >= NSEC_PER_SEC)
    {
        time.tv_nsec -= NSEC_PER_SEC;
        ++time.tv_sec;
    }
    clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&time,NULL);

}

int CanbusServoDriver::process_getErrorCode_buffer(unsigned int &errorCodeReturn)
{
    if( false==isInitialedOk)
    {
        return -1;
    }


    int tmpReturn;
    uint16_t errorCode;

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {


            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_receive(deviceId,busNumber,alias,0x2601,0,tmpDataOut,dataLength);
        errorCodeReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=4;
        tmpReturn=canbusDriver->sdoUpload_receive(deviceId,busNumber,alias,0x200B,0,tmpDataOut,dataLength);
        errorCodeReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::process_getPositionFeedback_buffer(int &positionFeedbackReturn)
{
    if( false==isInitialedOk)
    {
        return -1;
    }


    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=4;
        tmpReturn=canbusDriver->sdoUpload_receive(deviceId,busNumber,alias,0x6063,0,tmpDataOut,dataLength);
        positionFeedbackReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=4;
        tmpReturn=canbusDriver->sdoUpload_receive(deviceId,busNumber,alias,0x6063,0,tmpDataOut,dataLength);
        positionFeedbackReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::process_getStatusWord_buffer( int &statusWordReturn)
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_receive(deviceId,busNumber,alias,0x6041,0,tmpDataOut,dataLength);
        statusWordReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_receive(deviceId,busNumber,alias,0x6041,0,tmpDataOut,dataLength);
        statusWordReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::process_getSpeedFeedback_buffer(double &speedFeedbackReturn)
{

    if( false==isInitialedOk)
    {
        return -1;
    }

    uint16_t speedFeedback_16;

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {


            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=4;
        tmpReturn=canbusDriver->sdoUpload_receive(deviceId,busNumber,alias,0x606c,0,tmpDataOut,dataLength);
        double tmpVal=tmpDataOut/16.384;
        speedFeedbackReturn=tmpVal;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_receive(deviceId,busNumber,alias,0x606c,0,tmpDataOut,dataLength);
        double tmpVal=tmpDataOut*10000.0/60.0;//R/MIN
        speedFeedbackReturn=tmpVal;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::process_getCurrencyFeedback_buffer(double &currencyFeedbackReturn)
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    uint16_t currencyFeedback_16;

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {


            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_receive(deviceId,busNumber,alias,0x6078,0,tmpDataOut,dataLength);
        currencyFeedbackReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_receive(deviceId,busNumber,alias,0x6078,0,tmpDataOut,dataLength);
        currencyFeedbackReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::send_getErrorCode_command()
{
    if( false==isInitialedOk)
    {
        return -1;
    }


    int tmpReturn;

    switch(vendorId)
    {
    case E_VENDOR_ID_AI_KONG:
    {


        break;
    }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_noCareFeedback(deviceId,busNumber,alias,0x2601,0);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=4;
        tmpReturn=canbusDriver->sdoUpload_noCareFeedback(deviceId,busNumber,alias,0x200B,0);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::send_getPositionFeedback_command()
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    int tmpReturn;
    switch(vendorId)
    {
    case E_VENDOR_ID_AI_KONG:
    {


        break;
    }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=4;
        tmpReturn=canbusDriver->sdoUpload_noCareFeedback(deviceId,busNumber,alias,0x6063,0);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=4;
        tmpReturn=canbusDriver->sdoUpload_noCareFeedback(deviceId,busNumber,alias,0x6063,0);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::send_getStatusWord_command()
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    int tmpReturn;
    switch(vendorId)
    {
    case E_VENDOR_ID_AI_KONG:
    {
        // to do


        return 1;
    }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_noCareFeedback(deviceId,busNumber,alias,0x6041,0);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_noCareFeedback(deviceId,busNumber,alias,0x6041,0);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::send_getSpeedFeedback_command()
{


    if( false==isInitialedOk)
    {
        return -1;
    }

    uint16_t speedFeedback_16;

    int tmpReturn;
    switch(vendorId)
    {
    case E_VENDOR_ID_AI_KONG:
    {


        break;
    }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=4;
        tmpReturn=canbusDriver->sdoUpload_noCareFeedback(deviceId,busNumber,alias,0x606c,0);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_noCareFeedback(deviceId,busNumber,alias,0x606c,0);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::send_getCurrencyFeedback_command()
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    uint16_t currencyFeedback_16;

    int tmpReturn;
    switch(vendorId)
    {
    case E_VENDOR_ID_AI_KONG:
    {


        break;
    }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_noCareFeedback(deviceId,busNumber,alias,0x6078,0);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload_noCareFeedback(deviceId,busNumber,alias,0x6078,0);

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}


int CanbusServoDriver::servOnDriver()
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    int tmpReturn;

    switch(vendorId)
    {
    case E_VENDOR_ID_AI_KONG:
    {
        controlWord=eServoEnable;
        break;
    }
    case E_VENDOR_ID_KINCO:
    {
        if(E_MOTOR_CONTROL_CSP_MODE==controlMode)
        {
            tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6040,0,0x3f,2);
        }
        else
        {
            tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6040,0,0x0f,2);
        }

        controlWord=eServoEnable;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    case E_VENDOR_ID_MOTEC:
    {
        if(E_MOTOR_CONTROL_CSP_MODE==controlMode)
        {
            tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6040,0,0x3f,2);
        }
        else
        {
            tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6040,0,0x0f,2);
        }

        controlWord=eServoEnable;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    }


    return 1;
}

int CanbusServoDriver::servOffDriver()
{
    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }
    int tmpReturn;

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            controlWord=eServoOff;
            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6040,0,0x06,2);
        if(1==tmpReturn)
        {
            controlWord=eServoOff;
            return 1;
        }
        else
        {
            return tmpReturn;
        }

        break;
    }
    case E_VENDOR_ID_MOTEC:
    {
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6040,0,0x06,2);
        if(1==tmpReturn)
        {
            controlWord=eServoOff;
            return 1;
        }
        else
        {
            return tmpReturn;
        }

        break;
    }
    }


    return 1;
}

int CanbusServoDriver::quickStopDriver()
{
    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            controlWord=eServoQuickStop;
//            int resultKey=modbus_write_register(modbusMaster
//                                                ,regAdd.speedControlAdd,0);
            if(1==resultKey)
            {
//                qDebug()<<"quickStopDriver:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }
            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6040,0,0x0b,2);
        if(1==tmpReturn)
        {
            controlWord=eServoQuickStop;
            return 1;
        }
        else
        {
            return tmpReturn;
        }

        break;
    }
    case E_VENDOR_ID_MOTEC:
    {
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6040,0,0x0b,2);
        if(1==tmpReturn)
        {
            controlWord=eServoQuickStop;
            return 1;
        }
        else
        {
            return tmpReturn;
        }

        break;
    }
    }


    return 1;
}

int CanbusServoDriver::setDriverMode(E_MOTOR_CONTROL_MODE modeIn)
{

    if( false==isInitialedOk)
    {
        return -1;
    }

    controlMode=modeIn;


    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            //0=pwn 1=torque-loop 2=velocity-loop 3=position-loop
            int tmpMode;
            if(E_MOTOR_CONTROL_CSP_MODE==modeIn)
            {
                tmpMode=7;
            }
            else if(E_MOTOR_CONTROL_CSV_MODE==modeIn)
            {
                tmpMode=2;
            }
            else if(E_MOTOR_CONTROL_CST_MODE==modeIn)
            {
                tmpMode=1;
            }
            else
            {
                return -2;
            }

//            int resultKey=modbus_write_register(modbusMaster
//                                                ,regAdd.driverModelAdd,tmpMode);
            if(1==resultKey)
            {
                qDebug()<<"setDriverMode:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        signed char tmpCanOpenMode;
        if(E_MOTOR_CONTROL_CSP_MODE==modeIn)
        {
            tmpCanOpenMode=7;
        }
        else if(E_MOTOR_CONTROL_CSV_MODE==modeIn)
        {
//            tmpCanOpenMode=3;//带位置环控制的速度环，梯形加减速（步科）
            tmpCanOpenMode=-3;//直接速度环（步科）
        }
        else if(E_MOTOR_CONTROL_CST_MODE==modeIn)
        {
            tmpCanOpenMode=4;
        }
        else if(E_MOTOR_CONTROL_HOMING==modeIn)
        {
            tmpCanOpenMode=6;
        }
        else
        {
            return -2;
        }
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6060,0,tmpCanOpenMode,0x1);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    case E_VENDOR_ID_MOTEC:
    {
        signed char tmpCanOpenMode;
        if(E_MOTOR_CONTROL_CSP_MODE==modeIn)
        {
            tmpCanOpenMode=1;//轮廓位置模式，需要设置加速度等才能运行，如果用Ｓ曲线，上一条停止了，新的才能生效。Ｔ形随时可以切换。
        }
        else if(E_MOTOR_CONTROL_CSV_MODE==modeIn)
        {
            tmpCanOpenMode=3;//轮廓速度模式，有内部插补速度曲线。
        }
        else if(E_MOTOR_CONTROL_CST_MODE==modeIn)
        {
            tmpCanOpenMode=4;
        }
        else if(E_MOTOR_CONTROL_HOMING==modeIn)
        {
//            tmpCanOpenMode=6;
        }
        else
        {
            return -2;
        }
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6060,0,tmpCanOpenMode,0x2);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    case E_VENDOR_ID_ROBOTIQ:
    {
        //暂时仅支持速度模式
        return 1;
        break;
    }
    case E_VENDOR_ID_ROBOTIQ2MOTOR:
    {
        //暂时仅支持速度模式
        return 1;
        break;
    }
    case E_VENDOR_ID_CURTIS2MOTOR:
    {
        //暂时仅支持速度模式
        return 1;
        break;
    }
    case E_VENDOR_ID_XingSong2MOTOR:
    {
        //工作模式通过上位机配置软件进行配置。
        return 1;
        break;
    }
    case E_VENDOR_ID_AMC:
    {
        signed char tmpCanOpenMode;
        if(E_MOTOR_CONTROL_CSP_MODE==modeIn)
        {
//            tmpCanOpenMode=8;//csp mode
             tmpCanOpenMode=1;//pp mode
        }
        else if(E_MOTOR_CONTROL_CSV_MODE==modeIn)
        {
            tmpCanOpenMode=9;
        }
        else if(E_MOTOR_CONTROL_CST_MODE==modeIn)
        {
            tmpCanOpenMode=10;
        }
        else if(E_MOTOR_CONTROL_POSITION_MODE==modeIn)
        {
            tmpCanOpenMode=1;
        }
        else if(E_MOTOR_CONTROL_VELOCITY_MODE==modeIn)
        {
            tmpCanOpenMode=2;
        }
        else if(E_MOTOR_CONTROL_HOMING==modeIn)
        {
            tmpCanOpenMode=6;
        }
        else
        {
            return -2;
        }
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6060,0,tmpCanOpenMode,0x1);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    }

    return -1;

}

int CanbusServoDriver::setDriverNodeProtectTime(int msTime)
{

    if( false==isInitialedOk)
    {
        return -1;
    }

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {

                return -1;

            break;
        }
    case E_VENDOR_ID_KINCO:
    {

        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x100c,0,msTime,0x2);
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x100d,0,1,0x1);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    case E_VENDOR_ID_MOTEC:
    {

        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x100c,0,msTime,0x2);
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x100d,0,1,0x1);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }

        break;
    }
    case E_VENDOR_ID_ROBOTIQ:
    {
        return 1;
    }
    case E_VENDOR_ID_ROBOTIQ2MOTOR:
    {
        return 1;
    }
    case E_VENDOR_ID_CURTIS2MOTOR:
    {
        return 1;
    }
    case E_VENDOR_ID_XingSong2MOTOR:
    {
        return 1;
    }
    default:
    {
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x100c,0,msTime,0x2);
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x100d,0,1,0x1);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }

        break;
    }
    }

    return -1;

}

int CanbusServoDriver::setPosition(int positionIn, bool isWaitFeedback)
{

    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            //超时没有起作用，没发送数据的时候，没有停下来。
            uint16_t dataStream[2];
            dataStream[1] = positionIn & 0x0000ffff;
            dataStream[0] = ( positionIn & 0xffff0000 ) >> 16 ;
            //此处不支持写多个寄存器
//            int resultKey=modbus_write_registers(modbusMaster
//                                                ,regAdd.positionControlPositionHighAdd,2,dataStream);
            //write high value
//            int resultKey=modbus_write_register(modbusMaster
//                                                            ,regAdd.positionControlPositionHighAdd,dataStream[0]);

            if(-1!=resultKey)
            {
//                qDebug()<<"setPosition: modbus_write_register"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            //write low value
//            resultKey=modbus_write_register(modbusMaster
//                                                            ,regAdd.positionControlPositionLowAdd,
//                                            dataStream[1]);

            if(-1!=resultKey)
            {
//                qDebug()<<"setPosition: modbus_write_register"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        if(true==isWaitFeedback)
        {
            tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x607a,0,positionIn,4);
            realTimeSleep(D_SDO_WAIT_INTERVAL_TIME);
        }
        else
        {
            tmpReturn=canbusDriver->sdoDownload_noCareFeedback(deviceId,busNumber,alias,0x607a,0,positionIn,4);
        }

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }

    }
    case E_VENDOR_ID_MOTEC:
    {
        if(true==isWaitFeedback)
        {
            tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x607a,0,positionIn,4);
            realTimeSleep(D_SDO_WAIT_INTERVAL_TIME);
        }
        else
        {
            tmpReturn=canbusDriver->sdoDownload_noCareFeedback(deviceId,busNumber,alias,0x607a,0,positionIn,4);
        }

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }

    }

    }

    return -1;
}


int CanbusServoDriver::setPwm(int pwmIn)
{

    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }
    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
//            int resultKey=modbus_write_register(modbusMaster
//                                                ,regAdd.pwmControlAdd,pwmIn);
            if(1==resultKey)
            {
//                qDebug()<<"setPwm:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    }

    return -1;
}

int CanbusServoDriver::setSpeed(int speedIn,bool isWaitFeedback)
{

    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
        //speedIn=２４时，反馈速度约１２
        //可以设置t=１S后通信中断时，驱动器停止电机
        //霍尔无刷电机，４极，１２puls/r, 换相频率(１hz=3puls/s)(１hz=1puls/s)
        //寄存器0x0035=1时，换相频率无倍率
        //寄存器0x0035=0时，换相频率x0.1
            if(controlWord!=eServoEnable)
            {
                speedIn=0;
            }
//            int resultKey=modbus_write_register(modbusMaster
//                                                ,regAdd.speedControlAdd,10*speedIn);
            if(1==resultKey)
            {
//                qDebug()<<"setSpeed:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        double tmpVal=16.384*speedIn;
        speedIn=tmpVal;
        if(true==isWaitFeedback)
        {
            tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x60ff,0,speedIn,4);
            realTimeSleep(D_SDO_WAIT_INTERVAL_TIME);
        }
        else
        {
            tmpReturn=canbusDriver->sdoDownload_noCareFeedback(deviceId,busNumber,alias,0x60ff,0,speedIn,4);
        }

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        double tmpVal=60*speedIn/10000.0;
        speedIn=tmpVal;
        if(true==isWaitFeedback)
        {
            tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x60ff,0,speedIn,2);
            realTimeSleep(D_SDO_WAIT_INTERVAL_TIME);
        }
        else
        {
            tmpReturn=canbusDriver->sdoDownload_noCareFeedback(deviceId,busNumber,alias,0x60ff,0,speedIn,2);
        }

        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::setMaxCurrency(double ample)
{

    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
//            int resultKey=modbus_write_register(modbusMaster
//                                                ,regAdd.maxCurrencyAdd,ample*100);
            if(1==resultKey)
            {
//                qDebug()<<"setMaxCurrency:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6073,0,ample,2);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6073,0,ample,2);
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}


int CanbusServoDriver::setRatedCurrency(double ample)
{

    if( false==isInitialedOk)
    {
        return -1;
    }


//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
//            int resultKey=modbus_write_register(modbusMaster
//                                                ,regAdd.ratedCurrencyAdd,ample*100);
            if(1==resultKey)
            {
//                qDebug()<<"setRatedCurrency:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
//        tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6073,0,ample,0x20);
//        if(1!=tmpReturn)
//        {
//            return -1;
//        }
    }
    }

    return -1;
}

int CanbusServoDriver::changeDriverModbusId(int modbusId_new)
{
    //todo

    int tmpReturn;
    return 1;

}

int CanbusServoDriver::getErrorCode( int &errorCodeReturn)
{

    if( false==isInitialedOk)
    {
        return -1;
    }


//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }

    int tmpReturn;
    uint16_t errorCode;

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
//            int resultKey=modbus_read_registers(modbusMaster
//                                                ,regAdd.errorCodeAdd,1,&errorCode);
            if(1==resultKey)
            {
//                qDebug()<<"getErrorCode:modbus_write_registers sucess"<<resultKey;
                errorCodeReturn=errorCode;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload(deviceId,busNumber,alias,0x2601,0,tmpDataOut,dataLength);
        errorCodeReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload(deviceId,busNumber,alias,0x200B,0,tmpDataOut,dataLength);
        errorCodeReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::setControlWord(int controlWordIn, bool isWaitFeedback)
{
    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }
    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            // to do
            controlWord=(E_MOTOR_CMD)controlWordIn;
            return 1;
        }
    case E_VENDOR_ID_KINCO:
    {

        if(true==isWaitFeedback)
        {
            tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6040,0,controlWordIn,2);
            realTimeSleep(D_SDO_WAIT_INTERVAL_TIME);
        }
        else
        {
            tmpReturn=canbusDriver->sdoDownload_noCareFeedback(deviceId,busNumber,alias,0x6040,0,controlWordIn,2);
        }

        if(1==tmpReturn)
        {
            controlWord=(E_MOTOR_CMD)controlWordIn;
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {

        if(true==isWaitFeedback)
        {
            tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,0x6040,0,controlWordIn,2);
            realTimeSleep(D_SDO_WAIT_INTERVAL_TIME);
        }
        else
        {
            tmpReturn=canbusDriver->sdoDownload_noCareFeedback(deviceId,busNumber,alias,0x6040,0,controlWordIn,2);
        }

        if(1==tmpReturn)
        {
            controlWord=(E_MOTOR_CMD)controlWordIn;
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::getStatusWord(int& statusWordReturn)
{
    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }
    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            // to do
        //servo on state
        if(eServoEnable==controlWord)
        {
            statusWordReturn=eServoEnable;//与ethercat 一致
        }

        //servo off state
        if(eServoOff==controlWord)
        {
            statusWordReturn=eServoOff;//与ethercat 一致
        }

        //quick stop state
        if(eServoQuickStop==controlWord)
        {
            statusWordReturn=eServoQuickStop;//与ethercat 一致
        }

            return 1;
        }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload(deviceId,busNumber,alias,0x6041,0,tmpDataOut,dataLength);
        statusWordReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload(deviceId,busNumber,alias,0x6041,0,tmpDataOut,dataLength);
        statusWordReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::getPositionFeedback(int &positionFeedbackReturn)
{
    if( false==isInitialedOk)
    {
        return -1;
    }

//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }
    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            uint16_t dataStream[2];
//            int resultKey=modbus_read_registers(modbusMaster
//                                                ,regAdd.motorCurrentPositionFeedbackHighAdd,2,dataStream);
            if(-1!=resultKey)
            {
//                qDebug()<<"getPositionFeedback:modbus_read_registers sucess"<<resultKey;
                positionFeedbackReturn=dataStream[0]<<16;
                positionFeedbackReturn=positionFeedbackReturn+dataStream[1];
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=4;
        tmpReturn=canbusDriver->sdoUpload(deviceId,busNumber,alias,0x6063,0,tmpDataOut,dataLength);
        positionFeedbackReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=4;
        tmpReturn=canbusDriver->sdoUpload(deviceId,busNumber,alias,0x6063,0,tmpDataOut,dataLength);
        positionFeedbackReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;

}



int CanbusServoDriver::getSpeedFeedback(double &speedFeedbackReturn)
{


    if( false==isInitialedOk)
    {
        return -1;
    }
//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }
    uint16_t speedFeedback_16;

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            //霍尔无刷电机，４极，１２puls/r, 换相频率１hz=3puls/s １hz=1puls/s
            //寄存器0x0035=1时，换相频率无倍率
            //寄存器0x0035=0时，换相频率x0.1
//            int resultKey=modbus_read_registers(modbusMaster
//                                                ,regAdd.motorCurrentSpeedFeedbackAdd,1,&speedFeedback_16);
            if(1==resultKey)
            {
//                qDebug()<<"getSpeedFeedback:modbus_read_registers sucess"<<resultKey;
                int16_t *speedFeedback_s16=(int16_t*)&speedFeedback_16;
                speedFeedbackReturn=*speedFeedback_s16;
                speedFeedbackReturn=speedFeedbackReturn*0.1;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=4;
        tmpReturn=canbusDriver->sdoUpload(deviceId,busNumber,alias,0x606c,0,tmpDataOut,dataLength);
        double tmpVal=tmpDataOut/16.384;
        speedFeedbackReturn=tmpVal;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload(deviceId,busNumber,alias,0x606c,0,tmpDataOut,dataLength);
        double tmpVal=10000.0*tmpDataOut/60.0;
        speedFeedbackReturn=tmpVal;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}


int CanbusServoDriver::getCurrencyFeedback(double &currencyFeedbackReturn)
{

    if( false==isInitialedOk)
    {
        return -1;
    }
//    if(1!=checkModbusMaster())
//    {
//        return -100;
//    }
    uint16_t currencyFeedback_16;

    int tmpReturn;
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
//            int resultKey=modbus_read_registers(modbusMaster
//                                                ,regAdd.motorCurrentCurrencyFeedbackAdd,1,
//                                                &currencyFeedback_16);
            if(1==resultKey)
            {
//                qDebug()<<"getCurrencyFeedback:modbus_read_registers sucess"<<resultKey;
                currencyFeedbackReturn=(double)currencyFeedback_16/100.0;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    case E_VENDOR_ID_KINCO:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload(deviceId,busNumber,alias,0x6078,0,tmpDataOut,dataLength);
        currencyFeedbackReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
        break;
    }
    case E_VENDOR_ID_MOTEC:
    {
        int64_t tmpDataOut;
        int dataLength=2;
        tmpReturn=canbusDriver->sdoUpload(deviceId,busNumber,alias,0x6078,0,tmpDataOut,dataLength);
        currencyFeedbackReturn=tmpDataOut;
        if(1==tmpReturn)
        {
            return 1;
        }
        else
        {
            return tmpReturn;
        }
    }
    }

    return -1;
}

int CanbusServoDriver::getErrorCode_buffer(unsigned int &errorCodeReturn)
{
    errorCodeReturn=errorCode;
    return 1;
}

int CanbusServoDriver::getPositionFeedback_buffer(int &positionFeedbackReturn)
{
    positionFeedbackReturn=positionFeedback;
    return 1;
}

int CanbusServoDriver::getStatusWord_buffer(int &statusWordReturn)
{
    statusWordReturn=statusWord;
    return 1;
}

int CanbusServoDriver::getSpeedFeedback_buffer(double &speedFeedbackReturn)
{
    speedFeedbackReturn=speedFeedback;
    return 1;
}

int CanbusServoDriver::getCurrencyFeedback_buffer(double &currencyFeedbackReturn)
{
    currencyFeedbackReturn=currencyFeedback;
    return 1;
}

int CanbusServoDriver::getErrorCode_buffer_channel2(unsigned int &errorCodeReturn)
{
    errorCodeReturn=errorCode_channel2;
    return 1;
}

int CanbusServoDriver::getPositionFeedback_buffer_channel2(int &positionFeedbackReturn)
{
    positionFeedbackReturn=positionFeedback_channel2;
    return 1;
}

int CanbusServoDriver::getStatusWord_buffer_channel2(int &statusWordReturn)
{
    statusWordReturn=statusWord_channel2;
    return 1;
}

int CanbusServoDriver::getSpeedFeedback_buffer_channel2(double &speedFeedbackReturn)
{
    speedFeedbackReturn=speedFeedback_channel2;
    return 1;
}

int CanbusServoDriver::getCurrencyFeedback_buffer_channel2(double &currencyFeedbackReturn)
{
    currencyFeedbackReturn=currencyFeedback_channel2;
    return 1;
}

int CanbusServoDriver::initialDriverParameter()
{

    return 1;
}

bool CanbusServoDriver::isInitialOkStatus()
{
    return isInitialedOk;
}

int CanbusServoDriver::getVendorId()
{
    return vendorId;
}

#define D_SDO_TRY_MAX 5
int CanbusServoDriver::downloadServoSdoConfig()
{

    if( false==isInitialedOk)
    {
        return -1;
    }

    for(int i=0;i<sdoConfigList.size();i++)
    {
        for(int k=0;k<=D_SDO_TRY_MAX;k++)
        {
            int tmpReturn=canbusDriver->sdoDownload(deviceId,busNumber,alias,sdoConfigList[i].index
                                     ,sdoConfigList[i].subindex,sdoConfigList[i].value,sdoConfigList[i].byteLength);
            if(1==tmpReturn)
            {
                break;
            }
            else if(D_SDO_TRY_MAX==k)
            {
                qDebug()<<"downloadServoSdoConfig error,D_SDO_TRY_MAX"<<D_SDO_TRY_MAX
                       <<" alias"<<alias<<" index"<<sdoConfigList[i].index
                      <<" subindex"<<sdoConfigList[i].subindex
                     <<" byteLength"<<sdoConfigList[i].byteLength
                    <<" value"<<sdoConfigList[i].value;
                return -1;
            }
        }
    }



    return 1;
}

int CanbusServoDriver::process_can_buffer()
{

    int tmpResult;
    tmpResult=process_getErrorCode_buffer(errorCode);
    if(1!=tmpResult)
    {
        isUpdateSucess=false;
        qDebug()<<"process_getErrorCode_buffer failed,alias="<<alias;
        return -1;
    }
    tmpResult=process_getPositionFeedback_buffer(positionFeedback);
    if(1!=tmpResult)
    {
        qDebug()<<"process_getPositionFeedback_buffer failed,alias="<<alias;
        isUpdateSucess=false;
        return -2;
    }
    tmpResult=process_getStatusWord_buffer(statusWord);
    if(1!=tmpResult)
    {
        qDebug()<<"process_getStatusWord_buffer failed,alias="<<alias;
        isUpdateSucess=false;
        return -2;
    }
    tmpResult=process_getSpeedFeedback_buffer(speedFeedback);
    if(1!=tmpResult)
    {
        qDebug()<<"process_getSpeedFeedback_buffer failed,alias="<<alias;
        isUpdateSucess=false;
        return -3;
    }
    tmpResult=process_getCurrencyFeedback_buffer(currencyFeedback);
    if(1!=tmpResult)
    {
        qDebug()<<"process_getCurrencyFeedback_buffer failed,alias="<<alias;
        isUpdateSucess=false;
        return -5;
    }

    isUpdateSucess=true;
    return 1;
}

int CanbusServoDriver::process_can_buffer_pdo()
{
    if(E_VENDOR_ID_CURTIS2MOTOR==vendorId)
    {
        return process_can_buffer_pdo_curtis();
    }
    //bug 步科驱动器，ｐｄｏ返回的frameid全为１　？需要设置rpdo站号
    QVector<unsigned int> nodeIdOut;
    QVector<unsigned int> pdoIdOut;
    QVector<QVector<unsigned char> > dataListOut;

    canbusDriver->getPdoData_ch1(nodeIdOut,pdoIdOut,dataListOut);
//    qDebug()<<"getPdoData----------------------------------*********************************";
//    qDebug()<<"getPdoData nodeIdOut="<<nodeIdOut;
//    qDebug()<<"getPdoData pdoIdOut="<<pdoIdOut;
//    qDebug()<<"getPdoData dataListOut="<<dataListOut;

    bool isReceived=false;

    switch(vendorId)
    {
    case E_VENDOR_ID_KINCO:
    {
        for(int i=0;i<nodeIdOut.size();i++)
        {
            if(alias==nodeIdOut[i])
            {
                if(1==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        currencyFeedback=dataListOut[i][0];
                        currencyFeedback+=dataListOut[i][1]<<8;

                        errorCode=dataListOut[i][2];
                        errorCode+=dataListOut[i][3]<<8;

                        statusWord=dataListOut[i][4];
                        statusWord+=dataListOut[i][5]<<8;

                        int tmpValue;
                        tmpValue=dataListOut[i][6];//60f70010
                        tmpValue+=dataListOut[i][7]<<8;

                        batteryVoltage=tmpValue;

                    }
                }
                else if(2==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        positionFeedback=dataListOut[i][0];
                        positionFeedback+=dataListOut[i][1]<<8;
                        positionFeedback+=dataListOut[i][2]<<16;
                        positionFeedback+=dataListOut[i][3]<<24;

                        speedFeedback=dataListOut[i][4];
                        speedFeedback+=dataListOut[i][5]<<8;
                        speedFeedback+=dataListOut[i][6]<<16;
                        speedFeedback+=dataListOut[i][7]<<24;


                        speedFeedback=speedFeedback/16.384;

                        isReceived=true;

                    }
                }
            }

        }

        break;
    }
    case E_VENDOR_ID_MOTEC:
    {
        for(int i=0;i<nodeIdOut.size();i++)
        {
            if(alias==nodeIdOut[i])
            {
                if(1==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        currencyFeedback=dataListOut[i][0];
                        currencyFeedback+=dataListOut[i][1]<<8;

                        errorCode=dataListOut[i][2];
                        errorCode+=dataListOut[i][3]<<8;
                        errorCode+=dataListOut[i][4]<<16;
                        errorCode+=dataListOut[i][5]<<24;

                        statusWord=dataListOut[i][6];
                        statusWord+=dataListOut[i][7]<<8;

                    }
                }
                else if(2==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        positionFeedback=dataListOut[i][0];
                        positionFeedback+=dataListOut[i][1]<<8;
                        positionFeedback+=dataListOut[i][2]<<16;
                        positionFeedback+=dataListOut[i][3]<<24;

                        short tmpSpeed;

                        tmpSpeed=dataListOut[i][4];
                        tmpSpeed+=dataListOut[i][5]<<8;

                        speedFeedback=pulsePerRound_channel1*tmpSpeed/60.0;

                        isReceived=true;

                    }
                }
            }

        }

        break;
    }
    case E_VENDOR_ID_ROBOTIQ:
    {//前面低字节、后面高字节？
        for(int i=0;i<nodeIdOut.size();i++)
        {
            if(alias==nodeIdOut[i])
            {
                if(1==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        //statusWord 0 readyToswitchOn,1 switchOn,2 operationEnable,3 fault ,4 voltageEnable,5 quickStop,6 switchOnDisable
                        // 7 warnning ,8-15
                        int motorErrorCode;
                        motorErrorCode=dataListOut[i][0];
                        motorErrorCode+=dataListOut[i][1]<<8;
                        motorErrorCode+=dataListOut[i][2]<<16;
                        motorErrorCode+=dataListOut[i][3]<<24;


                        errorCode=dataListOut[i][4];
                        //合并电机错误
                        errorCode+=dataListOut[i][0]<<8;


                        if(0==errorCode)
                        {
                            statusWord=0x27;//使能状态
                        }
                        else if(16==errorCode)
                        {
                            statusWord=0;//servo off状态
                        }
                        else
                        {
                            statusWord=8;//急停外的其它报错
                        }



                    }
                }
                else if(2==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        currencyFeedback=dataListOut[i][0];
                        currencyFeedback+=dataListOut[i][1]<<8;
                        currencyFeedback+=dataListOut[i][2]<<16;
                        currencyFeedback+=dataListOut[i][3]<<24;

                        int tmpValue;
                        tmpValue=dataListOut[i][4];
                        tmpValue+=dataListOut[i][5]<<8;
                        tmpValue+=dataListOut[i][6]<<16;
                        tmpValue+=dataListOut[i][7]<<24;

                        batteryVoltage=tmpValue/10.0;
                        positionFeedback=0;
//                        qDebug()<<"batteryVoltage"<<batteryVoltage;


                    }
                }
                else if(3==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        int tmpSpeed;
                        tmpSpeed=dataListOut[i][0];
                        tmpSpeed+=dataListOut[i][1]<<8;
                        tmpSpeed+=dataListOut[i][2]<<16;
                        tmpSpeed+=dataListOut[i][3]<<24;


                        speedFeedback=tmpSpeed*pulsePerRound_channel1/60.0;//r/min转为pulse/s。舵轮使用１０２４线编码器

                        isReceived=true;

                    }
                }

            }

        }

        break;
    }
    case E_VENDOR_ID_ROBOTIQ2MOTOR:
    {
        //前面低字节、后面高字节
        for(int i=0;i<nodeIdOut.size();i++)
        {
            if(alias==nodeIdOut[i])
            {
                if(1==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        //statusWord 0 readyToswitchOn,1 switchOn,2 operationEnable,3 fault ,4 voltageEnable,5 quickStop,6 switchOnDisable
                        // 7 warnning ,8-15
                        int motorErrorCode;
                        motorErrorCode=dataListOut[i][0];
                        motorErrorCode+=dataListOut[i][1]<<8;
                        motorErrorCode+=dataListOut[i][2]<<16;
                        motorErrorCode+=dataListOut[i][3]<<24;
                        //暂时不处理电机２的错误

                        errorCode=dataListOut[i][4];
                        //合并电机错误
                        errorCode+=dataListOut[i][0]<<8;

                        errorCode_channel2=errorCode;


                        if(0==errorCode)
                        {
                            statusWord=0x27;//使能状态
                        }
                        else if(16==errorCode)
                        {
                            statusWord=0;//servo off状态
                        }
                        else
                        {
                            statusWord=8;//急停外的其它报错
                        }

                        statusWord_channel2=statusWord;



                    }
                }
                else if(2==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        currencyFeedback=dataListOut[i][0];
                        currencyFeedback+=dataListOut[i][1]<<8;
                        currencyFeedback+=dataListOut[i][2]<<16;
                        currencyFeedback+=dataListOut[i][3]<<24;

                        int tmpValue;
                        tmpValue=dataListOut[i][4];
                        tmpValue+=dataListOut[i][5]<<8;
                        tmpValue+=dataListOut[i][6]<<16;
                        tmpValue+=dataListOut[i][7]<<24;

                        batteryVoltage=tmpValue/10.0;
                        positionFeedback=0;
//                        qDebug()<<"batteryVoltage"<<batteryVoltage;


                    }
                }
                else if(3==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        int tmpSpeed;
                        tmpSpeed=dataListOut[i][0];
                        tmpSpeed+=dataListOut[i][1]<<8;
                        tmpSpeed+=dataListOut[i][2]<<16;
                        tmpSpeed+=dataListOut[i][3]<<24;


                        speedFeedback=tmpSpeed*pulsePerRound_channel1/60.0;//r/min转为pulse/s。舵轮使用１０２４线编码器
//                                qDebug()<<"pulsePerRound_channel1"<<pulsePerRound_channel1;
                        //暂时不处理电机２的错误


                    }
                }
                else if(4==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        positionFeedback_channel2=dataListOut[i][0];
                        positionFeedback_channel2+=dataListOut[i][1]<<8;
                        positionFeedback_channel2+=dataListOut[i][2]<<16;
                        positionFeedback_channel2+=dataListOut[i][3]<<24;

                        int tmpSpeed;
                        tmpSpeed=dataListOut[i][4];
                        tmpSpeed+=dataListOut[i][5]<<8;
                        tmpSpeed+=dataListOut[i][6]<<16;
                        tmpSpeed+=dataListOut[i][7]<<24;

                        speedFeedback_channel2=tmpSpeed*pulsePerRound_channel2/60.0;//r/min转为pulse/s。舵轮使用１０２４线编码器
//                                qDebug()<<"pulsePerRound_channel2"<<pulsePerRound_channel2;
                        isReceived=true;

                    }
                }

            }

        }

        break;
    }
    case E_VENDOR_ID_XingSong2MOTOR:
    {
        //前面低字节、后面高字节
        for(int i=0;i<nodeIdOut.size();i++)
        {
            if(alias==nodeIdOut[i])
            {
                if(1==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        currencyFeedback=dataListOut[i][0];
                        currencyFeedback+=dataListOut[i][1]<<8;


                        int tmpSpeed;
                        tmpSpeed=dataListOut[i][3];
                        tmpSpeed+=dataListOut[i][4]<<8;
                        if(1==dataListOut[i][2])
                        {
                            tmpSpeed=-tmpSpeed;
                        }
                        speedFeedback=tmpSpeed*pulsePerRound_channel1/60.0;//r/min转为pulse/s。舵轮使用１０２４线编码器

                        positionFeedback=dataListOut[i][5];
                        positionFeedback+=dataListOut[i][6]<<8;



                    }
                }
                else if(2==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        currencyFeedback_channel2=dataListOut[i][0];
                        currencyFeedback_channel2+=dataListOut[i][1]<<8;


                        int tmpSpeed;
                        tmpSpeed=dataListOut[i][3];
                        tmpSpeed+=dataListOut[i][4]<<8;
                        if(1==dataListOut[i][2])
                        {
                            tmpSpeed=-tmpSpeed;
                        }
                        speedFeedback_channel2=tmpSpeed*pulsePerRound_channel2/60.0;//r/min转为pulse/s。舵轮使用１０２４线编码器

                        positionFeedback_channel2=dataListOut[i][5];
                        positionFeedback_channel2+=dataListOut[i][6]<<8;


                    }
                }
                else if(3==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {

                        int tmpValue;
                        tmpValue=dataListOut[i][0];
                        tmpValue+=dataListOut[i][1]<<8;
                        batteryVoltage=tmpValue/10.0;
//                        qDebug()<<"batteryVoltage"<<batteryVoltage;

                        errorCode=dataListOut[i][4];
                        errorCode_channel2=errorCode;


                        if(0==errorCode && 1==controlWorld_xinsong)
                        {
                            statusWord=0x27;//使能状态
                        }
                        else if(0==errorCode && 1!=controlWorld_xinsong)
                        {
                            statusWord=0;//servo off状态
                        }
                        else
                        {
                            statusWord=8;//急停外的其它报错
                        }

                        statusWord_channel2=statusWord;

//                        isReceived=true;

                    }
                }
                else if(4==pdoIdOut[i])
                {
                    //0x1803sub2 必须设为１才行
                    if(8==dataListOut[i].size())
                    {

                        motor1ZeroStatus=dataListOut[i][0];
                        motor2ZeroStatus=dataListOut[i][1];

                        isReceived=true;

                    }
                }

            }

        }

        break;
    }
    case E_VENDOR_ID_AMC://单位换算查看说明书ｐａｇｅ290
    {
        //实际发送id=180+pdoId ,280+pdoId,380+pdoId,480+pdoId
        //tpod好像是固定不可修改的。速度模式和位置模式需要用不同的pdo通道. 位置用pdo2 速度用pdo4
        for(int i=0;i<nodeIdOut.size();i++)
        {
            if(alias==nodeIdOut[i])
            {
                if(2==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {
                        statusWord=dataListOut[i][0];
                        statusWord+=dataListOut[i][1]<<8;

                        positionFeedback=dataListOut[i][2];
                        positionFeedback+=dataListOut[i][3]<<8;
                        positionFeedback+=dataListOut[i][4]<<16;
                        positionFeedback+=dataListOut[i][5]<<24;
                        isReceived=true;


                    }
                }
                else if(4==pdoIdOut[i])
                {
                    if(8==dataListOut[i].size())
                    {

                        //速度单位DS1=pulse/s * 比例系数
                        //比例系数=2^17/(Kl * Ks)＝131072/(1*20000)=6.5536?
                        //Kl=0x2032.08h 0等效１倍
                        //Ks=65.536/0x20d8.24h 140000,手册查找switch frequency=20KHz
                        speedFeedback=dataListOut[i][2];
                        speedFeedback+=dataListOut[i][3]<<8;
                        speedFeedback+=dataListOut[i][4]<<16;
                        speedFeedback+=dataListOut[i][5]<<24;

                        currencyFeedback=0;

                        speedFeedback=speedFeedback/D_AMC_SPEED_RATIO;

                        isReceived=true;

                    }
                }
                else if(1==pdoIdOut[i])
                {

                }
            }
            else if(0x491-0x480==nodeIdOut[i])//可定义的节点id　0x491修改不了。
            {
                if(8==dataListOut[i].size())
                {
                    //驱动保护状态2002.2h 2byte、系统保护状态2002.3h 2byte、详细保护状态１(2002.4h 2byte)、状态２(2002.5h 2byte)

                    errorCode=dataListOut[i][0];
                    errorCode+=dataListOut[i][2]<<8;
                    errorCode+=dataListOut[i][3]<<16;
//                    errorCode+=dataListOut[i][4]<<24;//software disable

                    isReceived=true;

                }
            }
        }
        //修改状态字
        if(0!=errorCode)
        {
            statusWord=statusWord|8;
        }
        else
        {
//            statusWord=0x27;//使能状态
        }
        break;
    }
    }


    return isReceived;
}






